dual linear actuators w/ wedge

gistfile1.txt

int kinematicsForward(const double *joints,
                      EmcPose * pos,
                      const KINEMATICS_FORWARD_FLAGS * fflags,
                      KINEMATICS_INVERSE_FLAGS * iflags)
{
    // 2x2 Matrix components
    double a = tan(26 * M_PI/180);
    double b = -1.0 * tan(26 * M_PI/180);
    double c = 1.0;
    double d = 1.0;

    pos->tran.x = b * joints[0] + a * joints[2];
    pos->tran.z = joints[0]*2;

    return 0;
}

int kinematicsInverse(const EmcPose * pos,
                      double *joints,
                      const KINEMATICS_INVERSE_FLAGS * iflags,
                      KINEMATICS_FORWARD_FLAGS * fflags)
{ 
    // 2x2 Matrix components

    joints[2] = pos->tran.z/2 + pos->tran.x;
    joints[0] = pos->tran.z/2;

    return 0;
}

int kinematicsHome(EmcPose * world,
                   double *joint,
                   KINEMATICS_FORWARD_FLAGS * fflags,
                   KINEMATICS_INVERSE_FLAGS * iflags)
{
    *fflags = 0;
    *iflags = 0;

    return kinematicsForward(joint, world, fflags, iflags);
}

KINEMATICS_TYPE kinematicsType()
{
    return KINEMATICS_BOTH;
}